

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVOMIN 150  // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 600  // this is the 'maximum' pulse length count (out of 4096)

void setup() {
  Serial.begin(9600);
  Serial.println("16 channel Servo test!");

  pwm.begin();

  pwm.setPWMFreq(50);  // Analog servos run at ~60 Hz updates
}

// you can use this function if you'd like to set the pulse length in seconds
// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. its not precise!
void setServoPulse(uint8_t n, double pulse) {
  double pulselength;

  pulselength = 1000000;  // 1,000,000 us per second
  pulselength /= 60;      // 60 Hz
  Serial.print(pulselength);
  Serial.println(" us per period");
  pulselength /= 4096;  // 12 bits of resolution
  Serial.print(pulselength);
  Serial.println(" us per bit");
  pulse *= 1000;
  pulse /= pulselength;
  Serial.println(pulse);
  pwm.setPWM(n, 0, pulse);
}

void loop() {
  pwm.setPWM(0, 0, 120);
  pwm.setPWM(1, 0, 120);
  pwm.setPWM(2, 0, 120);
  pwm.setPWM(3, 0, 120);
  pwm.setPWM(4, 0, 120);
  pwm.setPWM(5, 0, 120);
  pwm.setPWM(6, 0, 120);
  pwm.setPWM(7, 0, 120);
  pwm.setPWM(8, 0, 120);
  pwm.setPWM(9, 0, 120);
  pwm.setPWM(10, 0, 120);
  pwm.setPWM(11, 0, 120);
  pwm.setPWM(12, 0, 120);
  pwm.setPWM(13, 0, 120);
  pwm.setPWM(14, 0, 120);
  pwm.setPWM(15, 0, 120);
  delay(10);
  // pwm.setPWM(0, 0, 200);
  // pwm.setPWM(1, 0, 200);
  // pwm.setPWM(2, 0, 200);
  // pwm.setPWM(3, 0, 200);
  // pwm.setPWM(4, 0, 200);
  // pwm.setPWM(5, 0, 200);
  // pwm.setPWM(6, 0, 200);
  // pwm.setPWM(7, 0, 200);
  // pwm.setPWM(8, 0, 200);
  // pwm.setPWM(9, 0, 200);
  // pwm.setPWM(10, 0, 200);
  // pwm.setPWM(11, 0, 200);
  // pwm.setPWM(12, 0, 200);
  // pwm.setPWM(13, 0, 200);
  // pwm.setPWM(14, 0, 200);
  // pwm.setPWM(15, 0, 200);
  // delay(1000);
  // Drive each servo one at a time
  //Serial.println(servonum);
  // for (uint16_t pulselen = SERVOMIN; pulselen < SERVOMAX; pulselen++) {
  //   pwm.setPWM(0, 0, pulselen);
  //   pwm.setPWM(1, 0, pulselen);
  //   pwm.setPWM(2, 0, pulselen);
  //   pwm.setPWM(3, 0, pulselen);
  //   pwm.setPWM(4, 0, pulselen);
  //   pwm.setPWM(5, 0, pulselen);
  //   pwm.setPWM(6, 0, pulselen);
  //   pwm.setPWM(7, 0, pulselen);
  //   pwm.setPWM(8, 0, pulselen);
  //   pwm.setPWM(9, 0, pulselen);
  //   pwm.setPWM(10, 0, pulselen);
  //   pwm.setPWM(11, 0, pulselen);
  //   pwm.setPWM(12, 0, pulselen);
  //   pwm.setPWM(13, 0, pulselen);
  //   pwm.setPWM(14, 0, pulselen);
  //   pwm.setPWM(15, 0, pulselen);
  // }
  // delay(500);
}

// #include <Arduino.h>
// #include <Servo.h>

// Servo myservo;

// void setup() {
//   myservo.attach(9);
// }

// void loop() {
//   myservo.write(0);
//   delay(10);
// }
